#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

using namespace std::placeholders;
using Image = sensor_msgs::msg::Image;
typedef message_filters::sync_policies::ApproximateTime<Image, Image> ApproximateSyncPolicy;

class DepthRGBSyncNode : public rclcpp::Node
{
public:
    DepthRGBSyncNode() : Node("depth_rgb_sync_node")
    {
        // 创建深度图和RGB图的订阅者
        depth_sub_ = std::make_shared<message_filters::Subscriber<Image>>(this, "/camera/aligned_depth_to_color/image_raw");
        rgb_sub_ = std::make_shared<message_filters::Subscriber<Image>>(this, "/camera/color/image_raw");

        // 使用近似时间同步器
        sync_ = std::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy>>(ApproximateSyncPolicy(10), *depth_sub_, *rgb_sub_);
        sync_->registerCallback(std::bind(&DepthRGBSyncNode::callback, this, _1, _2));
    }

private:
    // 定义近似时间同步策略
    void callback(const Image::ConstSharedPtr &depthImage, const Image::ConstSharedPtr &rgb_image)
    {
        RCLCPP_INFO(this->get_logger(), "Received synchronized depth and RGB image");

        // 这里处理同步后的深度图和RGB图
        (void)depthImage;
        (void)rgb_image;
    }

    std::shared_ptr<message_filters::Subscriber<Image>> depth_sub_;
    std::shared_ptr<message_filters::Subscriber<Image>> rgb_sub_;
    std::shared_ptr<message_filters::Synchronizer<ApproximateSyncPolicy>> sync_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<DepthRGBSyncNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}